using System;
using System.Collections.Generic;
using System.Text;

namespace CommonTypes
{
    public class Geometry
    {
        /// <summary>
        /// Convert an angle from Degrees to Radians
        /// </summary>
        /// <param name="x">Angle in Degrees</param>
        /// <returns>Angle in Radians</returns>
        public static double DegToRad(double x) { return x * System.Math.PI / 180.0; }


        /// <summary>
        /// Convert an angle from Radians to Degrees
        /// </summary>
        /// <param name="x">Angle in Radians</param>
        /// <returns>Angle in Degrees</returns>
        public static double RadToDeg(double x) { return x * 180.0 / System.Math.PI; }

        /// <summary>
        /// Function to Calculate the angle of three points
        ///		            Point1 (x1,y1)
        ///					/
        ///				   /
        ///		          /
        ///		         /
        /// Point2(x,y) /
        ///			   /-------------- Point3(x2,y2)	     
        /// </summary>
        /// <param name="x1">Point1 X</param>
        /// <param name="y1">Point1 Y</param>
        /// <param name="x">Point2 X</param>
        /// <param name="y">Point2 Y</param>
        /// <param name="x2">Point3 X</param>
        /// <param name="y2">Point3 Y</param>
        /// <returns></returns>
        public static double AngleBetweenPoints(double x1, double y1, double x, double y, double x2, double y2)
        {
            double z, s;
            double theta;
            double t1, t2;

            x1 -= x;
            y1 -= y;
            x2 -= x;
            y2 -= y;

            z = (x1 * y2) - (y1 * x2);
            s = (z >= 0.0) ? 1.0 : -1.0;
            t1 = System.Math.Sqrt(x1 * x1 + y1 * y1) * System.Math.Sqrt(x2 * x2 + y2 * y2);

            if (t1 == 0.0)
            {
                theta = 0.0;
            }
            else
            {
                t2 = ((x1 * x2) + (y1 * y2)) / t1;
                if ((t2 < -1) || (t2 > 1))
                {
                    theta = 0.0;
                }
                else
                {
                    theta = (s * System.Math.Acos(t2));
                }
            }

            return theta;
        }


        /// <summary>
        /// Check if a point (x,y) is inside a polygon (X[],Y[])
        /// </summary>
        /// <param name="X">Array of Polygon2D X points</param>
        /// <param name="Y">Array of Polygon2D Y points</param>
        /// <param name="x">Point x that will be checked</param>
        /// <param name="y">Point y that will be checked</param>
        /// <returns></returns>
        public static bool PointInsidePolygon(double[] X, double[] Y, double x, double y)
        {
            double  PI2 = 6.2;  //2* 3.14 magic number ;
            int i;
            double angle = 0.0;
            bool inside;

            // Implemented with the winding angle algorithm.
            // Just add up the angles between points
            for (i = 0; i < X.Length - 1; i++)
            {
                angle += AngleBetweenPoints(X[i], Y[i], x, y, X[i + 1], Y[i + 1]);
            }
            angle += AngleBetweenPoints(X[X.Length - 1], Y[X.Length - 1], x, y, X[0], Y[0]);

            // Allow for a bit of rounding
            // Ideally, angle should be 2*pi.
            if (System.Math.Abs(angle) > PI2 )
            {
                inside = true;
            }
            else
            {
                inside = false;
            }

            return (inside);

        }


        /// <summary>
        /// Return the Area of a Polygon2D  (Firts point and last 
        /// point in the arrays are the same)
        /// </summary>
        /// <param name="X">Array of Polygon2D X points</param>
        /// <param name="Y">Array of Polygon2D Y points</param>
        /// <returns></returns>
        public static double GetPolygonArea(double[] X, double[] Y)
        {
            double area = 0.0;

            for (int i = 0; i < X.Length - 1; i++)
            {
                area += X[i] * Y[i + 1] - X[i + 1] * Y[i];
            }

            area /= 2;


            return System.Math.Abs(area);

        }


        /// <summary>
        /// Determinant of an array
        /// |a1  a2|
        /// |      | = a1*b2 - a2*b1
        /// |b1  b2|
        /// </summary>
        /// <param name="a1">element 1,1</param>
        /// <param name="a2">element 1,2</param>
        /// <param name="b1">element 2,1</param>
        /// <param name="b2">element 2,2</param>
        /// <returns></returns>
        public static double E_Det(double a1, double a2, double b1, double b2)
        {
            return (a1 * b2 - b1 * a2);
        }


        /// <summary>
        /// Function to calculate where point(xp, yp) lies
        /// relative to line segment(xa, ya)(xb, yb)
        /// </summary>
        /// <param name="xa">segment Start X</param>
        /// <param name="ya">segment Start Y</param>
        /// <param name="xb">segment End X</param>
        /// <param name="yb">segment End Y</param>
        /// <param name="xp">Point X</param>
        /// <param name="yp">Point Y</param>
        /// <returns>
        /// 0 Point lies outside line segment
        /// 1 Point lies inside line segment
        ///</returns>
        public static int E_PntSgm(double xa, double ya, double xb, double yb, double xp, double yp)
        {
            double dab, dap, dbp, dpp;
            dab = E_SgmLen(xa, ya, xb, yb);
            dap = E_SgmLen(xp, yp, xa, ya);
            dbp = E_SgmLen(xp, yp, xb, yb);
            dpp = dap + dbp;
            if (E_Feq(dab, dpp) == 1) return (1);
            return (0);
        }


        /// <summary>
        /// Function to calculate the distance between two points
        /// </summary>
        /// <param name="x1">Start Point X</param>
        /// <param name="y1">Start Point Y</param>
        /// <param name="x2">End Point X</param>
        /// <param name="y2">End Point Y</param>
        /// <returns>distance between the points (The length of segment x1,y1 and x2,y2)</returns>
        public static double E_SgmLen(double x1, double y1, double x2, double y2)
        {
            double sqroot;
            sqroot = System.Math.Sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
            return (sqroot);
        }


        /// <summary>
        /// Check if two double are equal with tolerance 0.0001
        /// </summary>
        /// <param name="a1">first double</param>
        /// <param name="a2">second double</param>
        /// <returns>
        /// 1 if equal 
        /// 0 if not equal
        /// </returns>
        public static int E_Feq(double a1, double a2)
        {
            double a1d, a2d;
            a1d = (double)a1;
            a2d = (double)a2;
            if (System.Math.Abs(a1d - a2d) <= 0.0001f) return (1);
            return (0);
        }

        /// <summary>
        /// Specifies were the fuc... point is, or Something 
        /// </summary>
        public enum Type_of_Intersection
        {
            Prallel_segments,
            On_Line1,
            On_Line2,
            On_Both,
            On_Segment_Extent,
            On_SomethingMine_I_Realy_do_Not_remenber_in_this_fucking_mess
        }
        /// <summary>
        /// Function to calculate the intersection point of two
        ///	line segments and to determine the line segment on
        ///	which it exists.
        /// </summary>
        /// <param name="xd1">segment1 start x</param>
        /// <param name="yd1">segment1 start y</param>
        /// <param name="xf1">segment1 end x</param>
        /// <param name="yf1">segment1 end y</param>
        /// <param name="xd2">segment2 start x</param>
        /// <param name="yd2">segment2 start y</param>
        /// <param name="xf2">segment2 end x</param>
        /// <param name="yf2">segment2 end y</param>
        /// <param name="xi">the intersection points x</param>
        /// <param name="yi">the intersection points y</param>
        /// <returns>
        /// 0  Parallel line segments
        /// 1  Intersection point on line segment 1
        /// 2  Intersection point on line segment 2
        /// 3  Intersection point on both line segments
        /// 4  Intersection point out of line segments
        /// </returns>
        /// 


        public static Type_of_Intersection  E_SgmSec(double xd1, double yd1, double xf1, double yf1,
            double xd2, double yd2, double xf2, double yf2,
            out double xi, out double yi)
        {
            double dx1, dx2, dy1, dy2, d1, d2, det;
            int k1, k2;

            dy1 = yf1 - yd1;
            dx1 = xd1 - xf1;
            d1 = E_Det(xd1, xf1, yd1, yf1);
            dy2 = yf2 - yd2;
            dx2 = xd2 - xf2;
            d2 = E_Det(xd2, xf2, yd2, yf2);
            det = E_Det(dy1, dx1, dy2, dx2);
            if (System.Math.Abs(det) <= 0.05)
            {        /* Parellel line segments */
                xi = 0;
                yi = 0;
                return (Type_of_Intersection .Prallel_segments );
            }
            xi = E_Det(dx2, d2, dx1, d1) / det;
            yi = E_Det(dy1, d1, dy2, d2) / det;
            k1 = E_PntSgm(xd1, yd1, xf1, yf1, xi, yi);
            k2 = E_PntSgm(xd2, yd2, xf2, yf2, xi, yi);

            if (k1 + k2 == 0) return (Type_of_Intersection .On_Segment_Extent );
            if (k1 + k2 == 2) return (Type_of_Intersection .On_Both );
            if (k1 == 1) return (Type_of_Intersection .On_Line1 );
            else return (Type_of_Intersection .On_Line2 );
        }/***********************************************/


        /// <summary>
        /// The angle of a vector in radians
        /// </summary>
        /// <param name="x1">vector start x</param>
        /// <param name="y1">vector start y</param>
        /// <param name="x2">vector end x</param>
        /// <param name="y2">vector end y</param>
        /// <returns>The angle of a vector in radians</returns>
        public static double E_AngVect(double x1, double y1, double x2, double y2)
        {
            double A, A1, A2, Pi, Pi2, dx, dy, z;
            Pi = System.Math.PI;
            Pi2 = 2.0 * System.Math.PI;
            z = 0.0;
            dx = x2 - x1;
            dy = y2 - y1;
            if (dx == z)
                if (dy >= z)
                    A = Pi / 2.0;
                else A = Pi * 1.5; /* 90 ou 270 */
            else
                A = (System.Math.Atan(dy / dx));
            if (dx < z)
                A1 = Pi;
            else A1 = z;
            if ((dx > z) && (dy < z))
                A2 = Pi2;
            else A2 = z;
            A = A + A1 + A2;
            if (System.Math.Abs(A) > Pi2)
            {
                if (A >= z)
                    A = System.Math.Abs(A - Pi2);
                else A = -System.Math.Abs(A - Pi2);
            }
            return (A);
        }


        /// <summary>
        /// Function to calculate the coordinates of the line segment
        /// having coordinates x, y, length d, lying on line segment defined
        /// by(x1, y1) and(x2, y2) and is perpendicular at xa, ya.
        /// </summary>
        /// <param name="x1">segment start x</param>
        /// <param name="y1">segment start y</param>
        /// <param name="x2">segment end x</param>
        /// <param name="y2">segment end y</param>
        /// <param name="d">distance from segment</param>
        /// <param name="xa">points x on segment</param>
        /// <param name="ya">points y on segment</param>
        /// <param name="x">end points x of the perpendicular segment at xa,ya</param>
        /// <param name="y">end points y of the perpendicular segment at xa,ya</param>
        public static void E_PntVerSgm(double x1, double y1, double x2, double y2,
            double d, double xa, double ya, ref double x, ref double y)
        {
            double a, xb, yb;
            a = E_AngVect(x1, y1, x2, y2) + System.Math.PI / 2.0f;
            if (a > 2 * System.Math.PI)
                a = a - 2 * System.Math.PI;
            xb = xa + d * System.Math.Cos(a);
            yb = ya + d * System.Math.Sin(a);
            x = xb;
            y = yb;
        }


        /// <summary>
        /// Function to calculate the coordinates of the point 
        /// xa, xb lying on line segment defined
        /// by(x1, y1) and(x2, y2) and the perpendicular segment at x,y.
        /// </summary>
        /// <param name="x1">segment start x</param>
        /// <param name="y1">segment start y</param>
        /// <param name="x2">segment end x</param>
        /// <param name="y2">segment end y</param>
        /// <param name="x">points x on segment</param>
        /// <param name="y">points y on segment</param>
        /// <param name="xa">end points x of perpendicular segment</param>
        /// <param name="ya">end points y of perpendicular segment</param>
        /// <returns>
        /// 0 Point lies outside line segment
        /// 1 Point lies inside line segment
        /// </returns>
        public static int E_PntSgmVerSgm(double x1, double y1, double x2, double y2,
            double x, double y, ref double xa, ref double ya)
        {
            double a, d;

            a = E_AngVect(x1, y1, x2, y2) + System.Math.PI / 2.0;
            if (a > 2 * System.Math.PI)
                a = a - 2 * System.Math.PI;

            d = E_LenPntSgm(x, y, x1, y1, x2, y2);

            xa = x - d * System.Math.Cos(a);
            ya = y - d * System.Math.Sin(a);

            return E_PntSgm(x1, y1, x2, y2, xa, ya);
        }


        /// <summary>
        /// Function to calculate the distance of a point x, y
        /// from a line segment(xd1, yd1) , (xf1, yf1)
        /// </summary>
        /// <param name="x">points x</param>
        /// <param name="y">point y</param>
        /// <param name="xd1">segment start x</param>
        /// <param name="yd1">segment start y</param>
        /// <param name="xf1">segment end x</param>
        /// <param name="yf1">segment end y</param>
        /// <returns>Distance of x,y point</returns>
        public static double E_LenPntSgm(double x, double y, double xd1, double yd1, double xf1, double yf1)
        {
            double dist, dx, dy, root;
            dx = xf1 - xd1;
            dy = yd1 - yf1;
            if ((System.Math.Abs(dx) + System.Math.Abs(dy)) < 0.0001)
                dist = 0.0f;
            else
            {
                root = System.Math.Sqrt(dx * dx + dy * dy);
                dist = (dy * x + dx * y + xd1 * yf1 - yd1 * xf1) / root;
            }
            return (dist);
        }


        /// <summary>
        /// Function that calculates the coordinates of point(x, y)
        /// that lies on the line defined by(x1, y1), (x2, y2)
        /// </summary>
        /// <param name="x1">segment start x</param>
        /// <param name="y1">segment start y</param>
        /// <param name="x2">segment end x</param>
        /// <param name="y2">segment end y</param>
        /// <param name="d">distance from the start point</param>
        /// <param name="x">points x</param>
        /// <param name="y">points y</param>
        /// <returns>
        /// 1 Point is in line segment
        /// 0 Point is out of line segment
        /// </returns>
        public static int E_PntOnSgm(double x1, double y1, double x2, double y2, double d, ref  double x, ref double y)
        {
            double ratio;
            int Inline;
            ratio = d / E_SgmLen(x1, y1, x2, y2);
            x = x1 + (x2 - x1) * ratio;
            y = y1 + (y2 - y1) * ratio;
            Inline = E_PntSgm(x1, y1, x2, y2, x, y);
            return (Inline);
        }


        /// <summary>
        /// Function that defines a line segment defined by points
        /// (*xa, *ya) , (*xb, *yb) which is vertical on the line
        ///	segment defined by(x1, y1), (x2, y2) at a distance d
        /// having length l;
        /// </summary>
        /// <param name="x1">segment start x</param>
        /// <param name="y1">segment start y</param>
        /// <param name="x2">segment end x</param>
        /// <param name="y2">segment end y</param>
        /// <param name="d">distance from segment</param>
        /// <param name="xa">vertical segment start x</param>
        /// <param name="ya">vertical segment start y</param>
        /// <param name="xb">vertical segment end x</param>
        /// <param name="yb">vertical segment end y</param>
        /// <param name="l">segments length</param>
        /// <returns>
        /// 1 Vertical projection is in line segment
        ///	0 Vertical projection is out of line segment
        ///</returns>
        public static int E_SgmVerSgm(double x1, double y1, double x2, double y2, double d,
            ref double xa, ref double ya, ref double xb, ref double yb, double l)
        {
            int Inline;
            Inline = E_PntOnSgm(x1, y1, x2, y2, d, ref xa, ref ya);
            E_PntVerSgm(x1, y1, x2, y2, l, xa, ya, ref xb, ref yb);
            return (Inline);
        }



        /// <summary>
        /// GetRotationMatrix a point 
        /// </summary>
        /// <param name="x">points x</param>
        /// <param name="y">points y</param>
        /// <param name="angle">angle of rotation</param>
        public static void Rotate(ref double x, ref double y, double angle)
        {
            double cosinus = System.Math.Cos(angle * System.Math.PI / 180.0);
            double sinus = System.Math.Sin(angle * System.Math.PI / 180.0);


            double x1 = x * cosinus - y * sinus;
            double y1 = x * sinus + y * cosinus;

            x = x1;
            y = y1;

        }


        /// <summary>
        /// GetRotationMatrix a point around point cx,cy
        /// </summary>
        /// <param name="x">points x</param>
        /// <param name="y">points y</param>
        /// <param name="cx">center x of rotation</param>
        /// <param name="cy">center y of rotation</param>
        /// <param name="angle">angle of rotation</param>
        public static void Rotate(ref double x, ref double y, double cx, double cy, double angle)
        {
            double cosinus = System.Math.Cos(angle * System.Math.PI / 180.0);
            double sinus = System.Math.Sin(angle * System.Math.PI / 180.0);

            x -= cx;
            y -= cy;

            double x1 = x * cosinus - y * sinus;
            double y1 = x * sinus + y * cosinus;

            x = x1 + cx;
            y = y1 + cy;
        }

        public static double [] calcNormal(double[] r1, double[] r2)
        {

        // |a1  a2|
        // |      | = a1*b2 - a2*b1
        // |b1  b2|

            double[] normal = new double[3];
            normal[0] = E_Det(r1[1], r1[2], r2[1], r2[2]);
            normal[1] = -E_Det(r1[0], r1[2], r2[0], r2[2]);
            normal[2]= E_Det ( r1[0], r1[1], r2[0], r2[1]);
            return normal;

        }

        public static void normalize(ref double[] vector)
        {
            double length = vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2];
            length = System.Math.Sqrt(length);

            vector[0] = System.Math.Abs (vector[0]) / length;
            vector[1] = System.Math.Abs(vector[1] )/ length;
            vector[2] = System.Math.Abs(vector[2] )/ length;



        }


   //     public void  SegmentIntersection ()
       
            // dist3D_Segment_to_Segment():
//    Input:  two 3D line segments S1 and S2
//    Return: the shortest distance between S1 and S2






    }
}
